#include <opencv2/highgui.hpp>
#include "mbt/m_func.hh"
#include "mbt/bundel_hist.hh"
#include "mbt/tracker_edf.hh"

EDFTracker::EDFTracker(const cv::Matx33f& K, std::vector<Object3D*>& objects) 
: TrackerBase(K, objects) 
{

}

void DrawContour(std::vector<cv::Point>& pts, cv::Mat& buf) {
	for (int i = 0; i < pts.size(); ++i) {
		cv::circle(buf, pts[i], 1, cv::Scalar(255));
	}
}

void EDFTracker::ComputeJac(
	Object3D* object, 
	int m_id,
	const cv::Mat& frame,
	const cv::Mat& mask_map,
	const cv::Mat& depth_map,
	const cv::Mat& depth_inv_map,
	const cv::Mat& dt_map,
	cv::Matx66f& wJTJM,
	cv::Matx61f& JTM)
{
	float* depth_data = (float*)depth_map.ptr<float>();
	float* depth_inv_data = (float*)depth_inv_map.ptr<float>();
	uchar* frame_data = frame.data;
	uchar* mask_data = mask_map.data;
	float* dt_data = (float*)dt_map.ptr<float>();

	JTM = cv::Matx61f::zeros();
	wJTJM = cv::Matx66f::zeros();
	float* JT = JTM.val;
	float* wJTJ = wJTJM.val;

	float zf = view->getZFar();
	float zn = view->getZNear();
	cv::Matx33f K = view->GetCalibrationMatrix().get_minor<3, 3>(0, 0);
	float* K_inv_data =  K.inv().val;
	float fx = K(0,0);
	float fy = K(1,1);

	std::vector<cv::Point> ctr_pts;
	FindContours(mask_map, 2, true, ctr_pts);
#if 0
	cv::Mat buf(frame.size(), CV_8UC1, cv::Scalar(0));
	for (int i = 0; i < ctr_pts.size(); ++i) {
		cv::circle(buf, ctr_pts[i], 1, cv::Scalar(255));
	}
	cv::imshow("contour", buf);
	cv::waitKey();
#endif
	for (int r = 0; r < ctr_pts.size(); ++r) {
		int mx = ctr_pts[r].x;
		int my = ctr_pts[r].y;
		int zidx = my * depth_map.cols + mx;
		float ex = dt_data[zidx];
		
		if (mx < 1 || mx > frame.cols - 2 || my < 1 | my > frame.rows - 2)
			continue;

		//float we = tukey_weight(ex, 10);
		float we = 1.0f;

		float depth = 1.0f - depth_data[zidx];
		float D = 2.0f * zn * zf / (zf + zn - (2.0f * depth - 1.0) * (zf - zn));
		float Xc = D * (K_inv_data[0] * mx + K_inv_data[2]);
		float Yc = D * (K_inv_data[4] * my + K_inv_data[5]);
		float Zc = D;
		float Zc2 = Zc*Zc;

		float J[6];

		float difx = (dt_map.at<float>(my, mx + 1) - dt_map.at<float>(my, mx - 1)) / 2.0f;
		float dify = (dt_map.at<float>(my + 1, mx) - dt_map.at<float>(my - 1, mx)) / 2.0f;

		J[0] = difx * (-Xc*fx*Yc/Zc2) +     dify * (-fy -Yc*Yc*fy/Zc2);
		J[1] = difx * (fx + Xc*Xc*fx/Zc2) + dify * (Xc*Yc*fy/Zc2);
		J[2] = difx * (-fx*Yc/Zc)+          dify * (Xc*fy/Zc);
		J[3] = difx * (fx/Zc);
		J[4] =                              dify * (fy/Zc);
		J[5] = difx * (-Xc*fx/Zc2) +        dify * (-Yc*fy/Zc2);

		for (int n = 0; n < 6; n++) {
			JT[n] += we * ex * J[n];
		}

		for (int n = 0; n < 6; n++)
		for (int m = n; m < 6; m++) {
			wJTJ[n * 6 + m] += we * J[n] * J[m];
		}

		depth = 1.0f - depth_inv_data[zidx];
		D = 2.0f * zn * zf / (zf + zn - (2.0f * depth - 1.0) * (zf - zn));
		Xc = D * (K_inv_data[0] * mx + K_inv_data[2]);
		Yc = D * (K_inv_data[4] * my + K_inv_data[5]);
		Zc = D;
		Zc2 = Zc * Zc;

		J[0] = difx * (-Xc*fx*Yc/Zc2) +     dify * (-fy -Yc*Yc*fy/Zc2);
		J[1] = difx * (fx + Xc*Xc*fx/Zc2) + dify * (Xc*Yc*fy/Zc2);
		J[2] = difx * (-fx*Yc/Zc)+          dify * (Xc*fy/Zc);
		J[3] = difx * (fx/Zc);
		J[4] =															dify * (fy/Zc);
		J[5] = difx * (-Xc*fx/Zc2) +        dify * (-Yc*fy/Zc2);

		for (int n = 0; n < 6; n++) {
			JT[n] += we * ex * J[n];
		}

		for (int n = 0; n < 6; n++)
		for (int m = n; m < 6; m++) {
			wJTJ[n * 6 + m] += we * J[n] * J[m];
		}
	}

	for (int i = 0; i < wJTJM.rows; i++)
	for (int j = i + 1; j < wJTJM.cols; j++) {
		wJTJM(j, i) = wJTJM(i, j);
	}
}

void EDFTracker::Track(std::vector<cv::Mat>& imagePyramid, std::vector<Object3D*>& objects, int runs) {
#if 1
	for (int iter = 0; iter < runs * 4; iter++) {
		RunIteration(objects, imagePyramid, 2);
	}

	for (int iter = 0; iter < runs * 2; iter++) {
		RunIteration(objects, imagePyramid, 1);
	}

	for (int iter = 0; iter < runs * 1; iter++) {
		RunIteration(objects, imagePyramid, 0);
	}
#else
	RunIteration(objects, imagePyramid, 2);
	RunIteration(objects, imagePyramid, 1);
	RunIteration(objects, imagePyramid, 0);
#endif
}

#if 1
void EDFTracker::RunIteration(std::vector<Object3D*>& objects, const std::vector<cv::Mat>& imagePyramid, int level) {
	int width = view->GetWidth();
	int height = view->GetHeight();
	view->setLevel(level);
	int numInitialized = 0;
	for (int o = 0; o < objects.size(); o++) {
		if (!objects[o]->isInitialized())
			continue;

		numInitialized++;

		cv::Rect roi = Compute2DROI(objects[o], cv::Size(width / pow(2, level), height / pow(2, level)), 8);
		if (roi.area() == 0)
			continue;

		while (roi.area() < 3000 && level > 0) {
			level--;
			view->setLevel(level);
			roi = Compute2DROI(objects[o], cv::Size(width / pow(2, level), height / pow(2, level)), 8);
		}
	}

	view->setLevel(level);
	view->RenderSilhouette(std::vector<Model*>(objects.begin(), objects.end()), GL_FILL);
	cv::Mat depth_map = view->DownloadFrame(View::DEPTH);

	cv::Mat masks_map;
	if (numInitialized > 1) {
		masks_map = view->DownloadFrame(View::MASK);
	}	else {
		masks_map = depth_map;
	}

	//#define MULTI_OBJS
#ifdef MUTLI_OBJS
	int obj_size = objects.size();
#else
	int obj_size = 1;
#endif
	for (int o = 0; o < obj_size; o++) {
		if (!objects[o]->isInitialized())
			continue;

		cv::Rect roi = Compute2DROI(objects[o], cv::Size(width / pow(2, level), height / pow(2, level)), 8);
		if (roi.area() == 0)
			continue;

		cv::Mat roi_frame = imagePyramid[level](roi).clone();

		cv::Mat edge_map;
		DetectEdge(imagePyramid[level], edge_map);
		//DetectEdge(roi_frame, edge_map);

		cv::Mat wc_map;
		//FilterEdge(roi, imagePyramid[level], edge_map, wc_map);
#if 0
		cv::imshow("edge_map", wc_map);
		//cv::imshow("wc_map", wc_map);
		cv::waitKey();
#endif

//#if 0
		cv::Mat dt_map;
		//ComputeDistanceTransform(wc_map, dt_map);
		ComputeDistanceTransform(edge_map, dt_map);
#if 0
		cv::Mat distMap8u;
		cv::normalize(dt_map, distMap8u, 0, 255, cv::NORM_MINMAX, CV_8UC1);
		cv::imshow("dt_map", distMap8u);
		cv::waitKey();
#endif

		int m_id = (numInitialized <= 1) ? -1 : objects[o]->getModelID();
		cv::Mat mask_map;
		ConvertMask(masks_map, m_id, mask_map);
#if 0
		cv::imshow("mask_map", mask_map);
		cv::waitKey(0);
#endif

		view->RenderSilhouette(objects[o], GL_FILL, true);
		cv::Mat depth_inv_map = view->DownloadFrame(View::DEPTH);

		cv::Matx66f wJTJ;
		cv::Matx61f JT;
		ComputeJac(objects[o], m_id, imagePyramid[level], mask_map, depth_map, depth_inv_map, dt_map, wJTJ, JT);

		cv::Matx44f T_cm = Transformations::exp(-wJTJ.inv(cv::DECOMP_CHOLESKY)*JT)* objects[o]->getPose();
		objects[o]->setPose(T_cm);
//#endif
	}
}
#endif

static inline bool ptInImg(const cv::Point &pt, int h, int w)
{
	return pt.x >= 0 && pt.x < w&&pt.y >= 0 && pt.y < h;
}

static inline bool ptInImg(int x, int y, int h, int w)
{
	return x >= 0 && x < w&&y >= 0 && y < h;
}

static inline void computeEdgeNormal(int x, int y, uchar* imgData, int w, int h, int mstep, float *na) {
	int xl = x < 1 ? x : x - 1;
	int xr = x > w - 2 ? x : x + 1;
	int yu = y < 1 ? y : y - 1;
	int yd = y > h - 2 ? y : y + 1;

	float gx = (*(imgData + mstep*y + xr) - *(imgData + mstep*y + xl)) / (xr - xl);
	float gy = (*(imgData + mstep*yd + x) - *(imgData + mstep*yu + x)) / (yd - yu);
	// float gx = (imgGray.at<uchar>(y, xr) - imgGray.at<uchar>(y, xl)) / (xr - xl);
	// float gy = (imgGray.at<uchar>(yd, x) - imgGray.at<uchar>(yu, x)) / (yd - yu);

	float direction = atan2(gy, gx); // [-pi, pi]
	*na = direction < 0 ? direction + CV_PI : direction; // direction to orientation [0, pi]
}

static inline void computeEdgeProb(int x, int y, float na, int radius, const cv::Mat &img, 
	uchar *bgrImgPtr, int w, int h, int mstep, BundleHist* hists, uchar *p)
{
	int imgHeight = h;
	int imgWidth = w;
	float err = 0;
	float probFgLeft = 1;
	float probFgRight = 1;
	float probBgLeft = 1;
	float probBgRight = 1;

	int pointRx = 0;
	int pointRy = 0;
	int pointLx = 0;
	int pointLy = 0;

	// case 1
	if (na >= 0 && na <= CV_PI / 4)
	{
		float delta = tan(na);
		int y_r = 0;
		for (int x_r = 1; x_r < radius + 1; ++x_r)
		{
			err += delta;
			if (err > 0.5f)
			{
				y_r += 1;
				err -= 1;
			}

			pointRx = x+x_r;
			pointRy = y+y_r;
			pointLx = x-x_r;
			pointLy = y-y_r;

			if (ptInImg(pointRx, pointRy, imgHeight, imgWidth))
			{
				float ppf, ppb;
				uchar bi = *(bgrImgPtr + mstep * pointRy + 3 * pointRx);
				uchar gi = *(bgrImgPtr + mstep * pointRy + 3 * pointRx + 1);
				uchar ri = *(bgrImgPtr + mstep * pointRy + 3 * pointRx + 2);
				hists->GetPixelProb(ri, gi, bi, pointRx, pointRy, 0, ppf, ppb);
				probFgRight *= ppf;
				probBgRight *= ppb;
			}

			if (ptInImg(pointLx, pointLy, imgHeight, imgWidth))
			{
				float ppf, ppb;
				uchar bi = *(bgrImgPtr + mstep * pointLy + 3 * pointLx);
				uchar gi = *(bgrImgPtr + mstep * pointLy + 3 * pointLx + 1);
				uchar ri = *(bgrImgPtr + mstep * pointLy + 3 * pointLx + 2);
				hists->GetPixelProb(ri, gi, bi, pointLx, pointLy, 0, ppf, ppb);
				probFgLeft *= ppf;
				probBgLeft *= ppb;
			}
		}
	}

	// case 2
	if (na > CV_PI / 4 && na <= CV_PI / 2)
	{
		float delta;
		if (na > CV_PI / 2 - 0.01){
			delta = 0;
		} else{
			delta = 1 / tan(na);
		}

		int x_r = 0;
		for (int y_r = 1; y_r < radius + 1; ++y_r)
		{
			err += delta;
			if (err > 0.5f)
			{
				x_r += 1;
				err -= 1;
			}
			pointRx = x+x_r;
			pointRy = y+y_r;
			pointLx = x-x_r;
			pointLy = y-y_r;

			if (ptInImg(pointRx, pointRy, imgHeight, imgWidth))
			{
				float ppf, ppb;
				uchar bi = *(bgrImgPtr + mstep * pointRy + 3 * pointRx);
				uchar gi = *(bgrImgPtr + mstep * pointRy + 3 * pointRx + 1);
				uchar ri = *(bgrImgPtr + mstep * pointRy + 3 * pointRx + 2);
				hists->GetPixelProb(ri, gi, bi, pointRx, pointRy, 0, ppf, ppb);
				probFgRight *= ppf;
				probBgRight *= ppb;
			}

			if (ptInImg(pointLx, pointLy, imgHeight, imgWidth))
			{
				float ppf, ppb;
				uchar bi = *(bgrImgPtr + mstep * pointLy + 3 * pointLx);
				uchar gi = *(bgrImgPtr + mstep * pointLy + 3 * pointLx + 1);
				uchar ri = *(bgrImgPtr + mstep * pointLy + 3 * pointLx + 2);
				hists->GetPixelProb(ri, gi, bi, pointLx, pointLy, 0, ppf, ppb);
				probFgLeft *= ppf;
				probBgLeft *= ppb;
			}
		}
	}

	// case 3
	if (na > CV_PI / 2 && na <= 3 * CV_PI / 4)
	{
		float delta;
		if (na < CV_PI / 2 + 0.01){
			delta = 0;
		} else{
			delta = -1 / tan(na);
		}
		
		int x_r = 0;
		for (int y_r = 1; y_r < radius + 1; ++y_r)
		{
			err += delta;
			if (err > 0.5f)
			{
				x_r -= 1;
				err -= 1;
			}
			pointRx = x-x_r;
			pointRy = y-y_r;
			pointLx = x+x_r;
			pointLy = y+y_r;

			if (ptInImg(pointRx, pointRy, imgHeight, imgWidth))
			{
				float ppf, ppb;
				uchar bi = *(bgrImgPtr + mstep * pointRy + 3 * pointRx);
				uchar gi = *(bgrImgPtr + mstep * pointRy + 3 * pointRx + 1);
				uchar ri = *(bgrImgPtr + mstep * pointRy + 3 * pointRx + 2);
				hists->GetPixelProb(ri, gi, bi, pointRx, pointRy, 0, ppf, ppb);
				probFgRight *= ppf;
				probBgRight *= ppb;
			}

			if (ptInImg(pointLx, pointLy, imgHeight, imgWidth))
			{
				float ppf, ppb;
				uchar bi = *(bgrImgPtr + mstep * pointLy + 3 * pointLx);
				uchar gi = *(bgrImgPtr + mstep * pointLy + 3 * pointLx + 1);
				uchar ri = *(bgrImgPtr + mstep * pointLy + 3 * pointLx + 2);
				hists->GetPixelProb(ri, gi, bi, pointLx, pointLy, 0, ppf, ppb);
				probFgLeft *= ppf;
				probBgLeft *= ppb;
			}
		}
	}

	// case 4
	if (na > 3 * CV_PI / 4 && na <= CV_PI)
	{
		float delta = -tan(na);
		int y_r = 0;
		for (int x_r = 1; x_r < radius + 1; ++x_r)
		{
			err += delta;
			if (err > 0.5f)
			{
				y_r -= 1;
				err -= 1;
			}
			pointRx = x+x_r;
			pointRy = y+y_r;
			pointLx = x-x_r;
			pointLy = y-y_r;

			if (ptInImg(pointRx, pointRy, imgHeight, imgWidth))
			{
				float ppf, ppb;
				uchar bi = *(bgrImgPtr + mstep * pointRy + 3 * pointRx);
				uchar gi = *(bgrImgPtr + mstep * pointRy + 3 * pointRx + 1);
				uchar ri = *(bgrImgPtr + mstep * pointRy + 3 * pointRx + 2);
				hists->GetPixelProb(ri, gi, bi, pointRx, pointRy, 0, ppf, ppb);
				probFgRight *= ppf;
				probBgRight *= ppb;
			}

			if (ptInImg(pointLx, pointLy, imgHeight, imgWidth))
			{
				float ppf, ppb;
				uchar bi = *(bgrImgPtr + mstep * pointLy + 3 * pointLx);
				uchar gi = *(bgrImgPtr + mstep * pointLy + 3 * pointLx + 1);
				uchar ri = *(bgrImgPtr + mstep * pointLy + 3 * pointLx + 2);
				hists->GetPixelProb(ri, gi, bi, pointLx, pointLy, 0, ppf, ppb);
				probFgLeft *= ppf;
				probBgLeft *= ppb;
			}
		}
	}

	float pfb = probFgLeft*probBgRight;
	float pbf = probBgLeft*probFgRight;
	float pff = probFgLeft*probFgRight;
	float pbb = probBgLeft*probBgRight;

	float poe = std::max(pfb, pbf);
	float pnoe = std::max(pff, pbb);

	*p = poe > pnoe ? 255 : 0;
}

void EDFTracker::Filter(const cv::Mat &bgrImg, const cv::Mat &grayImg, cv::Mat cannyImg, cv::Mat &filteredEdge) {
	cv::Mat imgEdge = cannyImg;
	
	filteredEdge = cv::Mat::zeros(imgEdge.size(), CV_8UC1);

	uchar* grayData = (uchar*)grayImg.data;
	uchar* bgrImgData = (uchar*)bgrImg.data;
	int w = grayImg.cols;
	int h = grayImg.rows;
	int gray_mstep = grayImg.step1();
	int bgrImg_mstep = bgrImg.step1();

	for (int i = 0; i < imgEdge.rows; ++i) {
		const uchar *imgEdgePtr = imgEdge.ptr<uchar>(i);
		uchar *edgeMapPtr = filteredEdge.ptr<uchar>(i);
		
		for (int j = 0; j < imgEdge.cols; ++j) {
			if (imgEdgePtr[j]>0) {
				float na = 0;
				computeEdgeNormal(j, i, grayData, w, h, gray_mstep, &na);

				//computeEdgeProb(j, i, na, 3, bgrImg, bgrImgData, w, h, bgrImg_mstep, hists, edgeMapPtr + j);
			}
		}
	}
}

void EDFTracker::FilterEdge(const cv::Rect& roi, const cv::Mat& frame, const cv::Mat& edge_map, cv::Mat& wc_map) {
	cv::Mat gray;
	cv::cvtColor(frame, gray, CV_BGR2GRAY);
	Filter(frame, gray, edge_map, wc_map);
}

void EDFTracker::FindContours(const cv::Mat& projection_mask, int seg, bool all_contours, std::vector<cv::Point>& ctr_pts) {
	std::vector<std::vector<cv::Point> > contours;

	if (all_contours)
		cv::findContours(projection_mask, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
	else
		cv::findContours(projection_mask, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);

	for (int j = 0; j < contours.size(); ++j) {
		//if (contours[j].size() > 8) {
			for (int i = 0; i < contours[j].size(); i += seg) {
				ctr_pts.push_back(contours[j][i]);
			}
		//}
	}
}

void EDFTracker::ComputeDistanceTransform(cv::Mat& edge_map, cv::Mat& edf) {
	cv::distanceTransform(~edge_map, edf, CV_DIST_L2, 3);
}

void EDFTracker::Backproject(std::vector<cv::Point>& pt2ds, cv::Mat& depth_map, std::vector<cv::Vec3f>& pt3ds) {
	float zf = view->getZFar();
	float zn = view->getZNear();
	cv::Matx33f K = view->GetCalibrationMatrix().get_minor<3, 3>(0, 0);
	float* K_inv_data =  K.inv().val;
	float* depth_data = (float*)depth_map.ptr<float>();
	
	pt3ds.clear();
	for (int i = 0; i < pt2ds.size(); ++i) {
		int mx = pt2ds[i].x;
		int my = pt2ds[i].y;
		int zidx = my * depth_map.cols + mx;

		float depth = 1.0f - depth_data[zidx];
		float D = 2.0f * zn * zf / (zf + zn - (2.0f * depth - 1.0) * (zf - zn));
		float Xc = D * (K_inv_data[0] * mx + K_inv_data[2]);
		float Yc = D * (K_inv_data[4] * my + K_inv_data[5]);
		float Zc = D;
		pt3ds.push_back(cv::Vec3f(Xc, Yc, Zc));
	}
}

struct LMData {
	std::vector<cv::Vec3f> pt3ds;
	std::vector<cv::Vec3f> ipt3ds;
	cv::Matx44f Kmat;
	cv::Mat dt_map;
};


void JacFunc(float* p, float* j, int m, int n, void* adata);
void ErrFunc(float* p, float* e, int m, int n, void* adata);
#if 0
void EDFTracker::RunIteration(std::vector<Object3D*>& objects, const std::vector<cv::Mat>& imagePyramid, int level) {
	int width = view->GetWidth();
	int height = view->GetHeight();
	view->setLevel(level);
	int numInitialized = 0;
	for (int o = 0; o < objects.size(); o++) {
		if (!objects[o]->isInitialized())
			continue;

		numInitialized++;

		cv::Rect roi = Compute2DROI(objects[o], cv::Size(width / pow(2, level), height / pow(2, level)), 8);
		if (roi.area() == 0)
			continue;

		while (roi.area() < 3000 && level > 0) {
			level--;
			view->setLevel(level);
			roi = Compute2DROI(objects[o], cv::Size(width / pow(2, level), height / pow(2, level)), 8);
		}
	}

	view->setLevel(level);
	view->RenderSilhouette(std::vector<Model*>(objects.begin(), objects.end()), GL_FILL);
	cv::Mat depth_map = view->DownloadFrame(View::DEPTH);

	cv::Mat masks_map;
	if (numInitialized > 1) {
		masks_map = view->DownloadFrame(View::MASK);
	}	else {
		masks_map = depth_map;
	}

	//#define MULTI_OBJS
#ifdef MUTLI_OBJS
	int obj_size = objects.size();
#else
	int obj_size = 1;
#endif
	for (int o = 0; o < obj_size; o++) {
		if (!objects[o]->isInitialized())
			continue;

		cv::Rect roi = Compute2DROI(objects[o], cv::Size(width / pow(2, level), height / pow(2, level)), 8);
		if (roi.area() == 0)
			continue;

		cv::Mat roi_frame = imagePyramid[level](roi).clone();

		cv::Mat edge_map;
		DetectEdge(imagePyramid[level], edge_map);
		cv::Mat wc_map;
		FilterEdge(roi, imagePyramid[level], edge_map, wc_map);
#if 0
		cv::imshow("wc_map", wc_map);
		//cv::waitKey();
#endif
		cv::Mat dt_map;
		ComputeDistanceTransform(wc_map, dt_map);
#if 0
		cv::Mat distMap8u;
		cv::normalize(dt_map, distMap8u, 0, 255, cv::NORM_MINMAX, CV_8UC1);
		cv::imshow("dt_map", distMap8u);
		cv::waitKey();
#endif
		int m_id = (numInitialized <= 1) ? -1 : objects[o]->getModelID();
		cv::Mat mask_map;
		ConvertMask(masks_map, mask_map, m_id);

		std::vector<cv::Point> pt2ds;
		FindContours(mask_map, 1, true, pt2ds);

		std::vector<cv::Vec3f> pt3ds;
		Backproject(pt2ds, depth_map, pt3ds);

		view->RenderSilhouette(objects[o], GL_FILL, true);
		cv::Mat depth_inv_map = view->DownloadFrame(View::DEPTH);

		//std::vector<cv::Vec3f> ipt3ds;
		//Backproject(pt2ds, depth_inv_map, ipt3ds);

		LMData lm_data;
		lm_data.pt3ds = pt3ds;
		//lm_data.ipt3ds = ipt3ds;
		lm_data.Kmat = view->GetCalibrationMatrix();
		lm_data.dt_map = dt_map;

		//float mestimatorParams[2] = { (float)ME_TUKEY, (float)mestimatorThresh };
		float mestimatorParams[2] = { (float)ME_L2, 10.0f };
		int itx_max = 8;
		float dp[6] = { 0, 0, 0, 1E-4, 0, 0 };
		const float MU = 1E-4;
		const float STOP_THRESH = 1E-12;
		float opts[4] = { MU, STOP_THRESH, STOP_THRESH, STOP_THRESH }, info[11] = { 1E10 };
		SolveLM(ErrFunc, JacFunc, dp, NULL, 6, pt3ds.size(),
			mestimatorParams, itx_max, opts, info, NULL, &lm_data);

		cv::Matx44f T_cm = Transformations::exp(cv::Vec6f(dp)) * objects[o]->getPoseMatrix();
		objects[o]->setPose(T_cm);
	}
}
#endif
void ErrFunc(float* p, float* e, int m, int n, void* adata) {
	LMData *data = (LMData*)adata;
	const cv::Matx44f &Kmat = data->Kmat;
	const cv::Mat &dt_map = data->dt_map;
	const std::vector<cv::Vec3f> &pt3ds = data->pt3ds;
	const std::vector<cv::Vec3f> &ipt3ds = data->ipt3ds;

	cv::Matx44f pmat = Transformations::exp(cv::Vec6f(p));
	
	for (int i = 0; i < pt3ds.size(); ++i) {
		cv::Vec4f pth(pt3ds[i](0), pt3ds[i](1), pt3ds[i](2), 1);
		cv::Vec4f uvz = Kmat * pmat * pth;
		float z = uvz(2);
		if (z > 1E-12) {
			float x = static_cast<int>(uvz(0) / z);
			float y = static_cast<int>(uvz(1) / z);

			if (x >= 0 && x < dt_map.cols && y >= 1 && y < dt_map.rows) {
				e[i] = dt_map.at<float>(y, x);
			}	else {
				e[i] = 0;
			}
		}	else {
			e[i] = 0;
		}
	}
}

void JacFunc(float* p, float* jac, int m, int n, void* adata) {
	LMData *data = (LMData*)adata;
	const cv::Matx44f &Kmat = data->Kmat;
	const cv::Mat &dt_map = data->dt_map;
	const std::vector<cv::Vec3f> &pt3ds = data->pt3ds;
	const std::vector<cv::Vec3f> &ipt3ds = data->ipt3ds;

	cv::Matx44f pmat = Transformations::exp(cv::Vec6f(p));

	int k = 0;
	for (int i = 0; i < pt3ds.size(); ++i) {
		cv::Vec4f pth(pt3ds[i](0), pt3ds[i](1), pt3ds[i](2), 1);
		cv::Vec4f XYZ = pmat * pth;
		cv::Vec4f uvz = Kmat * XYZ;
		float z = uvz(2);
		if (z > 1E-12) {
			float mx = static_cast<int>(uvz(0) / z);
			float my = static_cast<int>(uvz(1) / z);
			
			if (mx >= 0 && mx < dt_map.cols && my >= 1 && my < dt_map.rows) {
				float difx = (dt_map.at<float>(my, mx + 1) - dt_map.at<float>(my, mx - 1)) / 2.0f;
				float dify = (dt_map.at<float>(my + 1, mx) - dt_map.at<float>(my - 1, mx)) / 2.0f;

				float Xc = XYZ(0), Yc = XYZ(1), Zc = XYZ(2), Zc2 = Zc * Zc;
				float fx = Kmat(0, 0), fy = Kmat(1, 1);

				jac[k++] = difx * (-Xc * fx * Yc / Zc2) + dify * (-fy - Yc * Yc * fy / Zc2);
				jac[k++] = difx * (fx + Xc * Xc * fx / Zc2) + dify * (Xc * Yc * fy / Zc2);
				jac[k++] = difx * (-fx * Yc / Zc) + dify * (Xc * fy / Zc);
				jac[k++] = difx * (fx / Zc);
				jac[k++] = dify * (fy / Zc);
				jac[k++] = difx * (-Xc * fx / Zc2) + dify * (-Yc * fy / Zc2);
			} else {
				jac[k++] = 0; jac[k++] = 0; jac[k++] = 0;
				jac[k++] = 0; jac[k++] = 0; jac[k++] = 0;
			}
		}	else {
			jac[k++] = 0; jac[k++] = 0; jac[k++] = 0;
			jac[k++] = 0; jac[k++] = 0; jac[k++] = 0;
		}
	}
}